//
// Created by zengyuhao on 25-4-28.
//

#ifndef ZORAL_CONVERT_POSE_METHOD_H
#define ZORAL_CONVERT_POSE_METHOD_H

#include "Eigen/Dense"

namespace ZORAL
{
#define CONV_TOL 1e-6

    Eigen::Matrix3d RotNorm(const Eigen::Matrix3d &rotation_matrix);
    Eigen::VectorXd Rot2Quat(const Eigen::Matrix3d &rotation_matrix);
    Eigen::VectorXd Trans2Quat(const Eigen::Matrix4d &transform_matrix);

    Eigen::Matrix3d Quat2Rot(const Eigen::Vector4d &quat);
    Eigen::Matrix4d Quat2Trans(const Eigen::VectorXd &vec7);

    Eigen::Vector4d EulerZYX2Quat(const Eigen::Vector3d &ypr);
    Eigen::Vector3d Quat2EulerZYX(const Eigen::Vector4d &quat_wxyz);

    Eigen::Vector3d Rot2EulerZYX(const Eigen::Matrix3d &R);
    Eigen::Matrix3d EulerZYX2Rot(const Eigen::Vector3d &ypr);
} // namespace ZORAL


#endif // ZORAL_CONVERT_POSE_METHOD_H
